#include "detection_headers.h"

using namespace NEWMAT;
using namespace DATMO;
using namespace pcl;

class Detection {
public:
        Detection();
        void RGB_Points_Callback(const sensor_msgs::PointCloud2::ConstPtr& msg);

        ros::NodeHandle n;
        ros::Publisher detected_objects_pub;
        string trial_folder_name;
        RobotState RobotStateData;
protected:
        int iteration;
        int it_detection_msg;
        StaticDetectionData SDData;
        DynamicDetectionData DDData;
};

//Class Initialization
Detection::Detection(){
  iteration = 1;
  it_detection_msg = 1;
  detected_objects_pub = n.advertise<datmo3::DetectedObjects_Msg>("datmo/detectedobjects",1000);
}

void Detection::RGB_Points_Callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  //convert msg to pcl point cloud
  pcl::PointCloud<pcl::PointXYZRGB> cloud;
  pcl::fromROSMsg(*msg,cloud);
  DistanceImage D = cl_Conversions::PointCloud2DistanceImage(cloud,&RobotStateData.tf_listener);

  cl_DynamicObjects DynamicObjects_act;

  SDData.LastLastDistImage = SDData.LastDistImage;
  SDData.LastDistImage = SDData.DistImage;
  SDData.DistImage = D;

    if(iteration == 1)
    {
      SDData.BackgroundRef.tf_Dref = Matrix(D.s_height,D.s_width);
      SDData.BackgroundRef.tf_Dref = D.tf_Distance;
      SDData.BackgroundRef.tf_Vref = Matrix(D.s_height,D.s_width);
      SDData.BackgroundRef.tf_Vref = 100000;
    }
    if (iteration > 4)
    {
      if(RobotStateData.b_isMovement == false) //static detection
      {
        if(!cl_CompareLaser::CompareLaser(SDData.DistImage,SDData.LastDistImage))
        {
          //Detect Objects

          DetectedObjects str_DetectedObjects = cl_ObjectDetection::ObjectDetection(SDData.DistImage,SDData.LastDistImage);

          //Update Reference
          SDData.BackgroundRef = cl_UpdateReference::UpdateReference(SDData.BackgroundRef,SDData.DistImage,SDData.LastDistImage,SDData.LastLastDistImage,str_DetectedObjects.m_Objects);

          //Objects2Real
          if (str_DetectedObjects.i_NumberOfObjects > 0)
          {
            ROS_INFO("Entra en paso a objetos reales");
            ros::Time actTime = ros::Time::now();
            DynamicObjects_act = cl_Objects2Real::Objects2Real(SDData.DistImage, str_DetectedObjects, SDData.BackgroundRef.tf_Dref,actTime);
            DynamicObjects_act.s_RobotID = RobotStateData.RobotID;
          }

        }
        else
        {
          Matrix auxMat(SDData.BackgroundRef.tf_Dref.Nrows(),SDData.BackgroundRef.tf_Dref.Ncols());
          auxMat = 0.0;
          auxMat.Release();
          SDData.BackgroundRef = cl_UpdateReference::UpdateReference(SDData.BackgroundRef,SDData.DistImage,SDData.LastDistImage,SDData.LastLastDistImage,auxMat);

        }
      }
      else //dynamic detection
      {

      }
      datmo3::DetectedObjects_Msg DetObjMsg = cl_Conversions::DetectedObjects2DetObjRosMsg(&DynamicObjects_act);
      it_detection_msg++;
      DetObjMsg.header.seq = it_detection_msg;
      DetObjMsg.RobotName = RobotStateData.RobotName;
      detected_objects_pub.publish(DetObjMsg);
    }

}


int main(int argc, char **argv)
{
  ros::init(argc,argv,"detection");
  ROS_INFO("PFUNCIONA!");
  Detection *controller = new Detection();


  //Time string for saving purposes
  time_t now = time(NULL);
  struct tm* now_time = gmtime(&now);
  std::stringstream time_str;
  time_str << now_time->tm_year+1900 <<"_" << now_time->tm_mon+1 <<"_";
  time_str << now_time->tm_mday <<"_" << now_time->tm_hour <<"_";
  time_str << now_time->tm_min <<"_" << now_time->tm_sec <<"_";
  //folder creation
  std::stringstream folder_name;
  folder_name << "./trial_" << time_str.str();
  controller->trial_folder_name = folder_name.str();
  string folder_string = folder_name.str();
  mkdir(folder_string.c_str(),S_IRWXU);

  ROS_INFO("PFUNCIONA2222!");
controller->n.getParam("Robot_ID",controller->RobotStateData.RobotID);
controller->n.getParam("Robot_name",controller->RobotStateData.RobotName);
ROS_INFO("PFUNCIONA!3333");
  //ROS Subscribers Creation

ros::Subscriber rgb_points_sub = controller->n.subscribe("camera/rgb/points",1000,&Detection::RGB_Points_Callback,controller);


  ros::spin();
  delete controller;


  return 0;


}
